#include <stdio.h>
#include <stdint.h>
#include <malloc.h>
#include "total_inventory.h"


P1010B_TanhTable tanhs;
DOBState  DOS;
ExpSmoothingTable Exp;
int main() {

    uint8_t high_byte = 0x2a;  // 高字节 0x06
    uint8_t low_byte = 0xbe;   // 低字节  0xa4

    uint8_t high_lq = 0;
    uint8_t low_lq = 0;
    P1010B_Split_int16(5.0,&high_byte,&low_byte,20);

//    P1010B_Split_int16(-10,&high_lq,&low_lq,20);

    // 使用 int16_t 来存储组合后的数据（16 位有符号整数）
    int16_t combined_data = Merge_uint8(high_byte,low_byte);


//    // 打印结果
    printf("Combined data (hex): 0x%02X\n", high_byte);  // 十六进制输出
    printf("Combined data (decimal): %d\n", combined_data);  // 十进制输出


    P1010B_init_tanh_table(&tanhs,-2,2,1000);

    float IS = compute_scaled_torque(&tanhs,5000,32768,0.6,40.0,0.0);
    printf("%f\r\n",IS);

    int32_t delta = 32000 - 20000;
    delta = ((delta + 16384) % 32768) - 16384;

    printf("%d\r\n",delta);

    float spedds = rpm_to_rads(177);
//    printf("%f\r\n",spedds);



    float ws = 15.0;
    float ws_pre = 10.0;




    float ssafaf = P1010B_CalculateCurrentFromTorque(20);   // 测试额定扭矩下的电流大小，23a为额定电流


//    printf("%f\r\n",ssafaf);


    init_exp_smoothing_table(&Exp, 4000.2, 32768, 1000);

    for(int i = 0;i <= 30000;i+=2000)
    {
        float f = exp_smoothing_lookup(&Exp, i ,0.0);
//        printf("%f\r\n",f);
    }

    CosLookupTable cos_table;
    SinLookupTable sin_table;
    init_cos_lookup(&cos_table, 360);
    init_sin_lookup(&sin_table, 360);

    float test_angles[] = {0, M_PI/4, M_PI/2, M_PI, 3*M_PI/2, 2*M_PI};
    for (int i = 0; i < sizeof(test_angles)/sizeof(test_angles[0]); i++) {
        float angle = test_angles[i];
        printf("cos(%.2f) = %.6f, sin(%.2f) = %.6f\n",
               angle, lookup_cos(&cos_table, angle),
               angle, lookup_sin(&sin_table, angle));
    }

    return 0;
}
